#include "Rope.h"



void	Rope::init(float mass, int points, Vector p0, Vector p1,
				bool bPos0Fixed, bool bPos1Fixed)

{
	mTotalMass = mass;
	mMass = mTotalMass / (float)points;

	Lr = (p1-p0).length() / 36.0f;
	ks = 30.0f;
	kd =  2.5f;

	Vector dir = (p1-p0).normalise();

	MassPoint mp;
	mp.velocity = Vector(0,0);
	mp.F = Vector(0,0);

	for (int i=0; i<points; i++) {

		Vector pos = p0 + i/(float)(points-1) * (p1-p0);	
		posData[i] = pos;
		mp.ptrPos = &posData[i];
		mPoints.push_back(mp);

	}
	mPoints[0].bFixed = bPos0Fixed;
	mPoints[points-1].bFixed = bPos1Fixed;
}


void	Rope::SpringForce(MassPoint& mp0, MassPoint& mp1, Vector* ret) 
{
	Vector p0 = *(mp0.ptrPos);
	Vector p1 = *(mp1.ptrPos);
	Vector v0 = mp0.velocity;
	Vector v1 = mp1.velocity;

	Vector L = p1-p0;
	float dL = L.length();

	*ret = (ks*(dL-Lr) - kd*((v0-v1).dot(L) / dL)) * L / dL;
}
	
void	Rope::simulate(float dt)
{
	Vector g(0.0f, -9.81f);
	Vector totalF, totalAccel;

	Vector p0, p1, v0, v1;

	externalForce = externalForce + mMass * g;


	for (unsigned int i=0; i<mPoints.size(); i++) 
	{
		if (i == 0 || i == mPoints.size() - 1)
			continue;

		p0 = *(mPoints[i].ptrPos);
		p1 = *(mPoints[i+1].ptrPos);

		v0 = mPoints[i].velocity;
		v1 = mPoints[i+1].velocity;

		Vector L = p1 - p0;
		float dL = L.length();

		Vector Fi, Fi_1;

		SpringForce(mPoints[i], mPoints[i+1], &Fi);
		SpringForce(mPoints[i-1], mPoints[i], &Fi_1);

		totalF = Fi - Fi_1 + externalForce;

		

		totalAccel = totalF / mMass;

		mPoints[i].velocity = mPoints[i].velocity + totalAccel * dt;
		*(mPoints[i].ptrPos) = *(mPoints[i].ptrPos) + mPoints[i].velocity * dt;
	}

	externalForce.Set(0,0);
}


Vector	Rope::getPointPos(int i)
{
	Vector p0 = *(mPoints[i].ptrPos);
	return p0;
}

void	Rope::addForce(Vector f) 
{
	externalForce = externalForce + f;
}

MassPoint&	Rope::point(int i)
{
	return mPoints[i];
}

unsigned int	Rope::numPoints()
{
	return mPoints.size();
}